#include "stdafx.h"
#include "Vector.h"
#include "Point.h"
#include <math.h>

using namespace aiRobots;

inline void RotateAlongX(mtype anglerad, mtype *destination)
{
	destination[0] = 1;
	destination[1] = 0;
	destination[2] = 0;
	destination[3] = 0;
	destination[4] = System::Math::Cos(anglerad);
	destination[5] = -System::Math::Sin(anglerad);
	destination[6] = 0;
	destination[7] = System::Math::Sin(anglerad);
	destination[8] = System::Math::Cos(anglerad);
}

inline void RotateAlongY(mtype anglerad, mtype *destination)
{
	destination[0] = System::Math::Cos(anglerad);
	destination[1] = 0;
	destination[2] = System::Math::Sin(anglerad);
	destination[3] = 0;
	destination[4] = 1;
	destination[5] = 0;
	destination[6] = -System::Math::Sin(anglerad);
	destination[7] = 0;
	destination[8] = System::Math::Cos(anglerad);
}

inline void RotateAlongZ(mtype anglerad, mtype *destination)
{
	destination[0] = System::Math::Cos(anglerad);
	destination[1] = -System::Math::Sin(anglerad);
	destination[2] = 0;
	destination[3] = System::Math::Sin(anglerad);
	destination[4] = System::Math::Cos(anglerad);
	destination[5] = 0;
	destination[6] = 0;
	destination[7] = 0;
	destination[8] = 1;
}

inline void RotateRPY(mtype* anglerad, mtype *destination)
{
	// R_rpy(phi, theta, psi) = R_z(psi) * R_y(theta) * R_x(phi)
	mtype cx = System::Math::Cos(anglerad[0]);
	mtype cy = System::Math::Cos(anglerad[1]);
	mtype cz = System::Math::Cos(anglerad[2]);
	mtype sx = System::Math::Sin(anglerad[0]);
	mtype sy = System::Math::Sin(anglerad[1]);
	mtype sz = System::Math::Sin(anglerad[2]);

	destination[0] = cy*cz;
	destination[1] = cz*sx*sy - cx*sz;
	destination[2] = sx*sz + cx*cz*sy;
	destination[3] = cy*sz;
	destination[4] = cx*cz + sx*sy*sz;
	destination[5] = cx*sy*sz - cz*sx;
	destination[6] = -sy;
	destination[7] = cy*sx;
	destination[8] = cx*cy;
}

inline void Transformation(mtype *rotation, mtype *position, mtype *destination)
{	
	// T = [  R,   | p;
	//      0,0,0, | 1]
	destination[0] = rotation[0];
	destination[1] = rotation[1];
	destination[2] = rotation[2];
	destination[3] = position[0];
	destination[4] = rotation[3];
	destination[5] = rotation[4];
	destination[6] = rotation[5];
	destination[7] = position[1];
	destination[8] = rotation[6];
	destination[9] = rotation[7];
	destination[10] = rotation[8];
	destination[11] = position[2];
	destination[12] = 0;
	destination[13] = 0;
	destination[14] = 0;
	destination[15] = 1;
}